﻿#ifndef __SOLVER_TYPES_H__
#define __SOLVER_TYPES_H__

#include "utils.h"

void direct_solver(const int nrows, double* A, double* rhs, TEST_TIME & record);
// 有复用的函数返回本次的分解结果
double* direct_reuse_solver(const int nrows, const double* A, double* rhs, TEST_TIME & record,
			const int last_nrows, const double* last_mem);
float*  iterat_solver		(const int nrows, const double* A, double* rhs, int pc_type, TEST_TIME & record,
			const int last_nrows, const float * last_mem);
double* direct_reuse_reorder_solver(const int nrows, const double* A, double* rhs, TEST_TIME & record,
			const int last_nrows, const double* last_mem, const std::vector<int> & prev_hfs);
float*  iterat_reorder_solver(const int nrows, const double* A, double* rhs, int pc_type, TEST_TIME & record,
			const int last_nrows, const float * last_mem, const std::vector<int> & prev_hfs);

template<typename data_t>
static void LU_update(int nrows, data_t* LU_old, int LU_lda,
		int rank, const data_t* L_1001, int L_1001_lda, const data_t* U_0110, int U_0110_lda)
{
#ifdef PROFILE
	auto beg = std::chrono::high_resolution_clock::now();
#endif
#ifdef USE_BLAS
	data_t* P_colM = new data_t[(long long)nrows * rank];
	data_t* Q_colM = new data_t[(long long)rank * nrows];
	for (int j = 0; j < rank; j++)
	for (int i = 0; i < nrows; i++) {
		P_colM[j * nrows + i] = - L_1001[j * L_1001_lda + i];
	}
	for (int j = 0; j < nrows; j++)
	for (int i = 0; i < rank; i++) {
		Q_colM[j * rank + i] =    U_0110[j * U_0110_lda + i];
	}
	data_t (*vec_dot)(const int, const data_t*, const int, const data_t*, const int) = nullptr;
	void (*gemm)(const CBLAS_LAYOUT, const CBLAS_TRANSPOSE, const CBLAS_TRANSPOSE, const int, const int, const int,
		const data_t, const data_t*, const int, const data_t*, const int, const data_t, data_t*, const int) = nullptr;
	void (*vec_scale)(const int N, const data_t alpha, data_t *X, const int incX) = nullptr;
	if constexpr (sizeof(data_t) == 4) {
		vec_dot = cblas_sdot;
		gemm = cblas_sgemm;
		vec_scale = cblas_sscal;
	} else if constexpr (sizeof(data_t) == 8) {
		vec_dot = cblas_ddot;
		gemm = cblas_dgemm;
		vec_scale = cblas_dscal;
	}
	for (int i = 0; i < nrows; i++) {
		data_t* Pi = P_colM + i		  ;
		data_t* Qi = Q_colM + i * rank;
		{// diagonal update
			data_t res = vec_dot(rank, Pi, nrows, Qi, 1);
			data_t val = LU_old[i * LU_lda + i];
			LU_old[i * LU_lda + i] += res;
			res = 1.0 / (val + res);
			vec_scale(rank, res, Qi, 1);
		}
		const int len = nrows - 1 - i;
		// L update
		data_t* P_ip1 = P_colM + i+1;
		data_t* L_new = LU_old + i*LU_lda + i+1;
		gemm(CblasColMajor, CblasNoTrans, CblasNoTrans, len, rank, 1,
			-1.0, L_new, LU_lda, Pi, nrows, 1.0, P_ip1, nrows);
		gemm(CblasColMajor, CblasNoTrans, CblasNoTrans, len, 1, rank,
			1.0, P_ip1, nrows, Qi, rank, 1.0, L_new, LU_lda);
		// U update
		data_t* Q_ip1 = Q_colM + (i+1)*rank;
		data_t* U_new = LU_old + (i+1)*LU_lda + i;
		gemm(CblasColMajor, CblasNoTrans, CblasNoTrans, 1, len, rank,
			1.0, Pi, nrows, Q_ip1, rank, 1.0, U_new, LU_lda);
		gemm(CblasColMajor, CblasNoTrans, CblasNoTrans, rank, len, 1,
			-1.0, Qi, rank, U_new, LU_lda, 1.0, Q_ip1, rank);
	}
#else
	data_t* P_rowM = new data_t[(long long)nrows * rank];
	data_t* Q_colM = new data_t[(long long)rank * nrows];
	#pragma omp parallel num_threads(gnt)
	{
		#pragma omp for schedule(static) nowait
		for (int i = 0; i < nrows; i++) {
			for (int j = 0; j < rank; j++)
				P_rowM[i * rank + j] = -L_1001[j * L_1001_lda + i];
		}
		#pragma omp for schedule(static)
		for (int j = 0; j < nrows; j++) {
			for (int i = 0; i < rank; i++)
				Q_colM[j * rank + i] = U_0110[j * U_0110_lda + i];
		}
	}
	// Standard recursive LU updating (a bit more favored by ColMajor layout)
#pragma omp parallel num_threads(gnt)
	{
#ifdef _WIN64
		int tid = omp_get_thread_num();
		SetThreadAffinityMask(GetCurrentThread(), DWORD_PTR{ 1 } << tid);
		SetThreadPriority(GetCurrentThread(), THREAD_PRIORITY_TIME_CRITICAL);
#endif
		for (int i = 0; i < nrows; i++) {
			data_t* Pi = P_rowM + i * rank;
			data_t* Qi = Q_colM + i * rank;
#pragma omp master
			{// diagonal update
				data_t res = 0.0;
				for (int k = 0; k < rank; k++)
					res += Pi[k] * Qi[k];

				data_t val = LU_old[i * LU_lda + i];
				LU_old[i * LU_lda + i] += res;

				res = 1.0 / (val + res);
				for (int k = 0; k < rank; k++)
					Qi[k] *= res;
			}
#pragma omp barrier
#pragma omp for schedule(static) nowait
			for (int j = i + 1; j < nrows; j++) {// L update
				data_t* Pj = P_rowM + j * rank;

				data_t val = LU_old[i * LU_lda + j];// L_new(j, i);
				data_t res = 0.0;
				for (int k = 0; k < rank; k++) {
					Pj[k] -= val * Pi[k];
					res += Pj[k] * Qi[k];
				}

				LU_old[i * LU_lda + j] += res;
			}
#pragma omp for schedule(static) // implicit barrier after this
			for (int j = i + 1; j < nrows; j++) {// U update
				data_t* Qj = Q_colM + j * rank;

				data_t val = LU_old[j * LU_lda + i];
				data_t res = 0.0;
				for (int k = 0; k < rank; k++)
					res += Pi[k] * Qj[k];

				LU_old[j * LU_lda + i] += res;

				res = -(val + res);
				for (int k = 0; k < rank; k++)
					Qj[k] += res * Qi[k];
			}
		}
	}
	delete[] P_rowM;
	delete[] Q_colM;
#endif
#ifdef PROFILE
	auto end = std::chrono::high_resolution_clock::now();
	double t = (std::chrono::duration_cast<std::chrono::nanoseconds>(end - beg)).count() * 1e-9;
	printf("LU_update %.6f s\n", t);
#endif
}

template<typename data_t>
void extend_full(const data_t* last_M, int last_nrows, data_t* this_M, int this_nrows)
{
	int info;
	int last_hf = last_nrows >> 1;
	int hf      = this_nrows >> 1;
	int nrows = this_nrows;

	if (last_M == nullptr) {// 上一次的是空的，直接分解
		if constexpr (sizeof(data_t) == 4) {
			info = LAPACKE_mkl_sgetrfnp(LAPACK_COL_MAJOR, nrows, nrows, this_M, nrows); assert(info == 0);// no pivot
		} else {
			info = LAPACKE_mkl_dgetrfnp(LAPACK_COL_MAJOR, nrows, nrows, this_M, nrows); assert(info == 0);// no pivot
		}
		return ;
	}

	// 根据上一次的结果计算本次的
	{
		data_t* rhs = new data_t[(long long)last_hf * (hf - last_hf) * 2];
		// U_0001:=L_0011^{-1}*B_00, U_0101:=L_0011^{-1}*B_01
		data_t* B_00 = this_M + last_hf * nrows;
		data_t* B_01 = this_M + (hf + last_hf) * nrows;
		#pragma omp parallel for schedule(static) num_threads(gnt)
		for (int j = 0; j < (hf - last_hf); j++)
		for (int i = 0; i < last_hf; i++) {
			rhs[ j                 * last_hf + i] = B_00[j * nrows + i];
			rhs[(j + hf - last_hf) * last_hf + i] = B_01[j * nrows + i];
		}
		if constexpr (sizeof(data_t) == 4) {
			cblas_strsm(CblasColMajor, CblasLeft, CblasLower, CblasNoTrans, CblasUnit,
				last_hf, (hf - last_hf) * 2, 1.0, last_M, last_nrows, rhs, last_hf);
		} else if constexpr (sizeof(data_t) == 8) {
			cblas_dtrsm(CblasColMajor, CblasLeft, CblasLower, CblasNoTrans, CblasUnit,
				last_hf, (hf - last_hf) * 2, 1.0, last_M, last_nrows, rhs, last_hf);
		}
		#pragma omp parallel for schedule(static) num_threads(gnt)
		for (int j = 0; j < (hf - last_hf); j++)
		for (int i = 0; i < last_hf; i++) {
			B_00[j * nrows + i] = rhs[j * last_hf + i];
			B_01[j * nrows + i] = rhs[(j + hf - last_hf) * last_hf + i];
		}
		// L_0010:=C_00*U_0011^{-1}, L_1010:=C_10*U_0011^{-1} 
		data_t* C_00 = this_M + last_hf;
		data_t* C_10 = this_M + last_hf + hf;
		#pragma omp parallel for schedule(static) num_threads(gnt)
		for (int j = 0; j < last_hf; j++)
		for (int i = 0; i < hf - last_hf; i++) {
			rhs[j * (hf - last_hf) * 2 + i] = C_00[j * nrows + i];
			rhs[j * (hf - last_hf) * 2 + i + hf - last_hf] = C_10[j * nrows + i];
		}
		if constexpr (sizeof(data_t) == 4) {
			cblas_strsm(CblasColMajor, CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit,
				(hf - last_hf) * 2, last_hf, 1.0, last_M, last_nrows, rhs, (hf - last_hf) * 2);
		} else if constexpr (sizeof(data_t) == 8) {
			cblas_dtrsm(CblasColMajor, CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit,
				(hf - last_hf) * 2, last_hf, 1.0, last_M, last_nrows, rhs, (hf - last_hf) * 2);
		}
		#pragma omp parallel for schedule(static) num_threads(gnt)
		for (int j = 0; j < last_hf; j++)
		for (int i = 0; i < hf - last_hf; i++) {
			C_00[j * nrows + i] = rhs[j * (hf - last_hf) * 2 + i];
			C_10[j * nrows + i] = rhs[j * (hf - last_hf) * 2 + i + hf - last_hf];
		}
		delete[] rhs;
	}

	{// 
		data_t* this_A00 = this_M;
		copy_mat(last_M, last_nrows, this_A00, nrows, last_hf, last_hf);// L_0000, U_0000
		//cblas_strsm(CblasColMajor, CblasLeft, CblasLower, CblasNoTrans, CblasUnit,
		//	last_hf, hf - last_hf, 1.0, last_M, last_nrows, this_A00 + last_hf * nrows, nrows);// U_0001
		//cblas_strsm(CblasColMajor, CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit,
		//	hf - last_hf, last_hf, 1.0, last_M, last_nrows, this_A00 + last_hf, nrows);// L_0010
		if constexpr (sizeof(data_t) == 4) {
			cblas_sgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, hf - last_hf, hf - last_hf, last_hf,
				-1.0, this_A00 + last_hf, nrows, this_A00 + last_hf * nrows, nrows, 1.0, this_A00 + last_hf * nrows + last_hf, nrows);
		} else if constexpr (sizeof(data_t) == 8) {
			cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, hf - last_hf, hf - last_hf, last_hf,
				-1.0, this_A00 + last_hf, nrows, this_A00 + last_hf * nrows, nrows, 1.0, this_A00 + last_hf * nrows + last_hf, nrows);
		}
#ifdef _OPEN
		int len = hf - last_hf;
		int ipiv = new int [len];
		if constexpr (sizeof(data_t) == 4) {
			LAPACK_sgetrf(&len, &len, this_A00 + last_hf * nrows + last_hf, &nrows, ipiv, &info); assert(info == 0);
		} else if constexpr (sizeof(data_t) == 8) {
			LAPACK_dgetrf(&len, &len, this_A00 + last_hf * nrows + last_hf, &nrows, ipiv, &info); assert(info == 0);
		}
		for (int i = 0; i < len; i++) assert(ipiv[i] == i+1);
		delete[] ipiv;
#else
		if constexpr (sizeof(data_t) == 4) {
			info = LAPACKE_mkl_sgetrfnp(CblasColMajor, hf - last_hf, hf - last_hf, this_A00 + last_hf * nrows + last_hf, nrows); assert(info == 0);
		} else if constexpr (sizeof(data_t) == 8) {
			info = LAPACKE_mkl_dgetrfnp(CblasColMajor, hf - last_hf, hf - last_hf, this_A00 + last_hf * nrows + last_hf, nrows); assert(info == 0);
		}
#endif
	}
	{// 
		data_t* this_A01 = this_M + hf * nrows;
		const data_t* L_0010 = this_M + last_hf;
		const data_t* L_0011 = this_M + last_hf + last_hf * nrows;
		copy_mat(last_M + last_hf * last_nrows, last_nrows, this_A01, nrows, last_hf, last_hf);// U_0100
		//cblas_strsm(CblasColMajor, CblasLeft, CblasLower, CblasNoTrans, CblasUnit,
		//	last_hf, hf - last_hf, 1.0, last_M, last_nrows, this_A01 + last_hf * nrows, nrows);// U_0101
		data_t* CD_01 = this_A01 + last_hf;
		if constexpr (sizeof(data_t) == 4) {
			cblas_sgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, hf - last_hf, hf, last_hf,
				-1.0, L_0010, nrows, this_A01, nrows, 1.0, CD_01, nrows);// [C_01, D_01] - L_0010*[U_0100, U_0101]
			cblas_strsm(CblasColMajor, CblasLeft, CblasLower, CblasNoTrans, CblasUnit,
				hf - last_hf, hf, 1.0, L_0011, nrows, CD_01, nrows);// [U_0110, U_0111]
		} else if constexpr (sizeof(data_t) == 8) {
			cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, hf - last_hf, hf, last_hf,
				-1.0, L_0010, nrows, this_A01, nrows, 1.0, CD_01, nrows);// [C_01, D_01] - L_0010*[U_0100, U_0101]
			cblas_dtrsm(CblasColMajor, CblasLeft, CblasLower, CblasNoTrans, CblasUnit,
				hf - last_hf, hf, 1.0, L_0011, nrows, CD_01, nrows);// [U_0110, U_0111]
		}
	}
	{// 
		data_t* this_A10 = this_M + hf;
		const data_t* U_0001 = this_M + last_hf * nrows;
		const data_t* U_0011 = this_M + last_hf * nrows + last_hf;
		copy_mat(last_M + last_hf, last_nrows, this_A10, nrows, last_hf, last_hf);// L_1000
		//cblas_strsm(CblasColMajor, CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit,
		//	hf - last_hf, last_hf, 1.0, last_M, last_nrows, this_A10 + last_hf, nrows);// L_1010
		data_t* BD_10 = this_A10 + last_hf * nrows;
		if constexpr (sizeof(data_t) == 4) {
			cblas_sgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, hf, hf - last_hf, last_hf,
				-1.0, this_A10, nrows, U_0001, nrows, 1.0, BD_10, nrows);
			cblas_strsm(CblasColMajor, CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit,
				hf, hf - last_hf, 1.0, U_0011, nrows, BD_10, nrows);// [L_1001, L_1011]
		} else if constexpr (sizeof(data_t) == 8) {
			cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, hf, hf - last_hf, last_hf,
				-1.0, this_A10, nrows, U_0001, nrows, 1.0, BD_10, nrows);
			cblas_dtrsm(CblasColMajor, CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit,
				hf, hf - last_hf, 1.0, U_0011, nrows, BD_10, nrows);// [L_1001, L_1011]
		} 
	}
	{// 
		data_t* this_A11 = this_M + hf * nrows + hf;
		data_t* B_11 = this_A11 + last_hf * nrows;
		const data_t* L_100x = this_M + hf;
		const data_t* U_01x1 = this_M + (hf + last_hf) * nrows;
		data_t* C_11 = this_A11 + last_hf;
		const data_t* L_101x = this_M + hf + last_hf;
		const data_t* U_01x0 = this_M + hf * nrows;
		data_t* D_11 = this_A11 + last_hf * nrows + last_hf;
		copy_mat(last_M + last_hf * last_nrows + last_hf, last_nrows, this_A11, nrows, last_hf, last_hf);// L_11_t, U_11_t
		LU_update(last_hf, this_A11, nrows, hf - last_hf, L_100x + last_hf * nrows, nrows, U_01x0 + last_hf, nrows);
		if constexpr (sizeof(data_t) == 4) {
			cblas_sgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, last_hf, hf - last_hf, hf,
				-1.0, L_100x, nrows, U_01x1, nrows, 1.0, B_11, nrows);
			cblas_strsm(CblasColMajor, CblasLeft, CblasLower, CblasNoTrans, CblasUnit,
				last_hf, hf - last_hf, 1.0, this_A11, nrows, B_11, nrows);
			
			cblas_sgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, hf - last_hf, last_hf, hf,
				-1.0, L_101x, nrows, U_01x0, nrows, 1.0, C_11, nrows);
			cblas_strsm(CblasColMajor, CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit,
				hf - last_hf, last_hf, 1.0, this_A11, nrows, C_11, nrows);
			cblas_sgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, hf - last_hf, hf - last_hf, hf + last_hf,
				-1.0, L_101x, nrows, U_01x1, nrows, 1.0, D_11, nrows);
		} else if constexpr (sizeof(data_t) == 8) {
			cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, last_hf, hf - last_hf, hf,
				-1.0, L_100x, nrows, U_01x1, nrows, 1.0, B_11, nrows);
			cblas_dtrsm(CblasColMajor, CblasLeft, CblasLower, CblasNoTrans, CblasUnit,
				last_hf, hf - last_hf, 1.0, this_A11, nrows, B_11, nrows);
			
			cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, hf - last_hf, last_hf, hf,
				-1.0, L_101x, nrows, U_01x0, nrows, 1.0, C_11, nrows);
			cblas_dtrsm(CblasColMajor, CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit,
				hf - last_hf, last_hf, 1.0, this_A11, nrows, C_11, nrows);
			cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, hf - last_hf, hf - last_hf, hf + last_hf,
				-1.0, L_101x, nrows, U_01x1, nrows, 1.0, D_11, nrows);
		}

#ifdef _OPEN
		int len = hf - last_hf;
		int ipiv = new int [len];
		if constexpr (sizeof(data_t) == 4) {
			LAPACK_sgetrf(&len, &len, D_11, &nrows, ipiv, &info); assert(info == 0);
		} else if constexpr (sizeof(data_t) == 8) {
			LAPACK_dgetrf(&len, &len, D_11, &nrows, ipiv, &info); assert(info == 0);
		}
		for (int i = 0; i < len; i++) assert(ipiv[i] == i+1);
		delete[] ipiv;
#else
		if constexpr (sizeof(data_t) == 4) {
			info = LAPACKE_mkl_sgetrfnp(CblasColMajor, hf - last_hf, hf - last_hf, D_11, nrows); assert(info == 0);
		} else if constexpr (sizeof(data_t) == 8) {
			info = LAPACKE_mkl_dgetrfnp(CblasColMajor, hf - last_hf, hf - last_hf, D_11, nrows); assert(info == 0);
		}
#endif
	}
}

template<typename data_t>
void extend_block(const data_t* last_M, int same_len, data_t* this_M, int this_lda, int* ipiv = nullptr) {
	int info;

	if (last_M == nullptr) {
		if constexpr (sizeof(data_t) == 4) {
			info = LAPACKE_mkl_sgetrfnp(LAPACK_COL_MAJOR, this_lda, this_lda, this_M, this_lda); assert(info == 0);// no pivot
		} else {
			info = LAPACKE_mkl_dgetrfnp(LAPACK_COL_MAJOR, this_lda, this_lda, this_M, this_lda); assert(info == 0);// no pivot
		}
		return ;
	}
	// 左上角块
	copy_mat(last_M, same_len, this_M, this_lda, same_len, same_len);
	
	data_t* A01 = this_M + same_len * this_lda;
	data_t* A10 = this_M + same_len;
	data_t* A11 = A01 + same_len;
	if constexpr (sizeof(data_t) == 4) {
		// 右上角块
		cblas_strsm(CblasColMajor, CblasLeft, CblasLower, CblasNoTrans, CblasUnit,
			same_len, this_lda - same_len, 1.0, this_M, this_lda, A01, this_lda);
		// 左下角块
		cblas_strsm(CblasColMajor, CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit,
			this_lda - same_len, same_len, 1.0, this_M, this_lda, A10, this_lda);
		// 右下角块
		cblas_sgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, this_lda - same_len, this_lda - same_len, same_len,
			-1.0, A10, this_lda, A01, this_lda, 1.0, A11, this_lda);
	} else if constexpr (sizeof(data_t) == 8) {
		// 右上角块
		cblas_dtrsm(CblasColMajor, CblasLeft, CblasLower, CblasNoTrans, CblasUnit,
			same_len, this_lda - same_len, 1.0, this_M, this_lda, A01, this_lda);
		// 左下角块
		cblas_dtrsm(CblasColMajor, CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit,
			this_lda - same_len, same_len, 1.0, this_M, this_lda, A10, this_lda);
		// 右下角块
		cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, this_lda - same_len, this_lda - same_len, same_len,
			-1.0, A10, this_lda, A01, this_lda, 1.0, A11, this_lda);
	}
	if (ipiv == nullptr) {// 不做 pivot
#ifdef _OPEN
		int len = this_lda - same_len;
		int* ipiv = new int [len];
		if constexpr (sizeof(data_t) == 4) {
			LAPACK_sgetrf(&len, &len, A11, &this_lda, ipiv, &info); assert(info == 0);
		} else if constexpr (sizeof(data_t) == 8) {
			LAPACK_dgetrf(&len, &len, A11, &this_lda, ipiv, &info); assert(info == 0);
		}
		for (int i = 0; i < len; i++) assert(ipiv[i] == i+1);
		delete[] ipiv;
#else
		if constexpr (sizeof(data_t) == 4) {
			info = LAPACKE_mkl_sgetrfnp(CblasColMajor, this_lda - same_len, this_lda - same_len, A11, this_lda); assert(info == 0);
		} else if constexpr (sizeof(data_t) == 8) {
			info = LAPACKE_mkl_dgetrfnp(CblasColMajor, this_lda - same_len, this_lda - same_len, A11, this_lda); assert(info == 0);
		}
#endif
	} else {
		if constexpr (sizeof(data_t) == 4) {
			info = LAPACKE_sgetrf(CblasColMajor, this_lda - same_len, this_lda - same_len, A11, this_lda, ipiv + same_len); assert(info == 0);
		} else if constexpr (sizeof(data_t) == 8) {
			info = LAPACKE_dgetrf(CblasColMajor, this_lda - same_len, this_lda - same_len, A11, this_lda, ipiv + same_len); assert(info == 0);
		}
		// 调整ipiv中的顺序
		for (int i = same_len; i < this_lda; i++)
			ipiv[i] += same_len;
	}
}

template<typename data_t>
void extend_block_lda(data_t* memory, const int last_nrows, const int nrows, int lda, int* ipiv = nullptr) {
	int info;
	assert(memory != nullptr);
	if (last_nrows == 0) {
		if constexpr (sizeof(data_t) == 4) {
			info = LAPACKE_mkl_sgetrfnp(LAPACK_COL_MAJOR, nrows, nrows, memory, lda); assert(info == 0);// no pivot
		} else {
			info = LAPACKE_mkl_dgetrfnp(LAPACK_COL_MAJOR, nrows, nrows, memory, lda); assert(info == 0);// no pivot
		}
		return ;
	}
	// 左上角块已经在内了
	const int delta = nrows - last_nrows;
	data_t* A01 = memory + last_nrows * lda;
	data_t* A10 = memory + last_nrows;
	data_t* A11 = A01 + last_nrows;
	if constexpr (sizeof(data_t) == 4) {
		// 右上角块
		cblas_strsm(CblasColMajor, CblasLeft, CblasLower, CblasNoTrans, CblasUnit,
			last_nrows, delta, 1.0, memory, lda, A01, lda);
		// 左下角块
		cblas_strsm(CblasColMajor, CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit,
			delta, last_nrows, 1.0, memory, lda, A10, lda);
		// 右下角块
		cblas_sgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, delta, delta, last_nrows,
			-1.0, A10, lda, A01, lda, 1.0, A11, lda);
	} else if constexpr (sizeof(data_t) == 8) {
		// 右上角块
		cblas_dtrsm(CblasColMajor, CblasLeft, CblasLower, CblasNoTrans, CblasUnit,
			last_nrows, delta, 1.0, memory, lda, A01, lda);
		// 左下角块
		cblas_dtrsm(CblasColMajor, CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit,
			delta, last_nrows, 1.0, memory, lda, A10, lda);
		// 右下角块
		cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, delta, delta, last_nrows,
			-1.0, A10, lda, A01, lda, 1.0, A11, lda);
	}
	if (ipiv == nullptr) {// 不做 pivot
#ifdef _OPEN
		int* ipiv = new int [delta];
		if constexpr (sizeof(data_t) == 4) {
			LAPACK_sgetrf(&delta, &delta, A11, &lda, ipiv, &info); assert(info == 0);
		} else if constexpr (sizeof(data_t) == 8) {
			LAPACK_dgetrf(&delta, &delta, A11, &lda, ipiv, &info); assert(info == 0);
		}
		for (int i = 0; i < len; i++) assert(ipiv[i] == i+1);
		delete[] ipiv;
#else
		if constexpr (sizeof(data_t) == 4) {
			info = LAPACKE_mkl_sgetrfnp(CblasColMajor, delta, delta, A11, lda); assert(info == 0);
		} else if constexpr (sizeof(data_t) == 8) {
			info = LAPACKE_mkl_dgetrfnp(CblasColMajor, delta, delta, A11, lda); assert(info == 0);
		}
#endif
	} else {
		if constexpr (sizeof(data_t) == 4) {
			info = LAPACKE_sgetrf(CblasColMajor, delta, delta, A11, lda, ipiv + last_nrows); assert(info == 0);
		} else if constexpr (sizeof(data_t) == 8) {
			info = LAPACKE_dgetrf(CblasColMajor, delta, delta, A11, lda, ipiv + last_nrows); assert(info == 0);
		}
		// 调整ipiv中的顺序
		for (int i = last_nrows; i < nrows; i++)
			ipiv[i] += last_nrows;
	}
}

template<typename data_t>
void inverse_of_lower_unit(const data_t* known, const int known_lda, data_t* res, const int res_lda, const int nrows)
{
	// size_t tot_elems = (size_t) nrows * (size_t) nrows;
	// for (size_t i = 0; i < tot_elems; i++) res[i] = 0.0;// 初始化置零

	for (int i = 0; i < nrows; i++) res[i * res_lda + i] = 1.0;// 先求对角元

	for (int d = 1; d < nrows; d++) {
		for (int i = d; i < nrows; i++) {
			int j = i - d;
			data_t tmp = - known[j * known_lda + i];
			for (int k = j + 1; k < i; k++) tmp -= known[k * known_lda + i] * res[j * res_lda + k];
			res[j * res_lda + i] = tmp;
		}
	}
}

template<typename data_t>
void inverse_of_upper_nonunit(const data_t* known, const int known_lda, data_t* res, const int res_lda, const int nrows)
{
	// size_t tot_elems = (size_t) nrows * (size_t) nrows;
	// for (size_t i = 0; i < tot_elems; i++) res[i] = 0.0;// 初始化置零

	for (int i = 0; i < nrows; i++) res[i * res_lda + i] = 1.0 / known[i * known_lda + i];// 先求对角元
	
	for (int d = 1; d < nrows; d++) {
		for (int j = d; j < nrows; j++) {
			int i = j - d;
			data_t tmp = 0.0;
			for (int k = i + 1; k <= j; k++) tmp -= known[k * known_lda + i] * res[j * res_lda + k];
			res[j * res_lda + i] = tmp / known[i * known_lda + i];
		}
	}
}

#endif